﻿using System;
using System.Diagnostics.Contracts;

namespace MathService.Vectors
{
    public struct Basis3D
    {
        private static readonly Func<double, double> sin = Math.Sin;
        private static readonly Func<double, double> cos = Math.Cos;

        public static readonly Basis3D Evclid = new Basis3D(
            1, 0, 0,
            0, 1, 0,
            0, 0, 1);

        public static Basis3D Scale(double kx, double ky, double kz)
        {
            return new Basis3D(
                kx, 0, 0,
                0, ky, 0,
                0, 0, kz);
        }

        public static Basis3D RotateOX(double Angle, bool Positive = true)
        {
            return new Basis3D(
                1, 0, 0,
                0, cos(Angle), (Positive ? -1 : 1) * sin(Angle),
                0, (Positive ? 1 : -1) * sin(Angle), cos(Angle));
        }

        public static Basis3D RotateOY(double Angle, bool Positive = true)
        {
            return new Basis3D(
                cos(Angle), 0, (Positive ? -1 : 1) * sin(Angle),
                0, 1, 0,
                (Positive ? 1 : -1) * sin(Angle), 0, cos(Angle));
        }

        public static Basis3D RotateOZ(double Angle, bool Positive = true)
        {
            return new Basis3D(
                cos(Angle), (Positive ? -1 : 1) * sin(Angle), 0,
                (Positive ? 1 : -1) * sin(Angle), cos(Angle), 0,
                0, 0, 1);
        }

        public static Basis3D Rotate(double alpha, double betta, double gamma)
        {
            var sin_a = sin(alpha);
            var cos_a = cos(alpha);

            var sin_b = sin(betta);
            var cos_b = cos(betta);

            var sin_g = sin(gamma);
            var cos_g = cos(gamma);

            return new Basis3D(
                        cos_a * cos_b, cos_g * sin_b * sin_g - sin_a * cos_g, cos_a * sin_b * cos_g + sin_a * sin_g,
                        sin_a * cos_b, sin_a * sin_b * sin_g + cos_a * cos_g, sin_a * sin_b * cos_g - cos_a * sin_g,
                        -sin_b, cos_b * sin_g, cos_b * cos_g);
        }

        public static Basis3D Rotate(Vector3D v, double thetta)
        {
            var sin_t = sin(thetta);
            var cos_t = cos(thetta);
            var cos_t1 = 1 - cos_t;

            var x = v.X;
            var y = v.Y;
            var z = v.Z;

            var xy = x * y;
            var xz = x * z;
            var yz = y * z;

            var sin_tx = sin_t * x;
            var sin_ty = sin_t * y;
            var sin_tz = sin_t * z;

            return new Basis3D(
                cos_t + cos_t1 * x * x, cos_t1 * xy - sin_tz, cos_t1 * xz + sin_ty,
                cos_t1 * xy + sin_tz, cos_t + cos_t1 * y * y, cos_t1 * yz - sin_tx,
                cos_t1 * xz - sin_ty, cos_t1 * yz + sin_tx, cos_t + cos_t1 * z * z);
        }

        private double f_xx;
        private double f_xy;
        private double f_xz;
        private double f_yx;
        private double f_yy;
        private double f_yz;
        private double f_zx;
        private double f_zy;
        private double f_zz;

        public double xx { get { return f_xx; } set { f_xx = value; } }
        public double xy { get { return f_xy; } set { f_xy = value; } }
        public double xz { get { return f_xz; } set { f_xz = value; } }

        public double yx { get { return f_yx; } set { f_yx = value; } }
        public double yy { get { return f_yy; } set { f_yy = value; } }
        public double yz { get { return f_yz; } set { f_yz = value; } }

        public double zx { get { return f_zx; } set { f_zx = value; } }
        public double zy { get { return f_zy; } set { f_zy = value; } }
        public double zz { get { return f_zz; } set { f_zz = value; } }

        public Basis3D(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz)
        {
            f_xx = xx; f_xy = xy; f_xz = xz;
            f_yx = yx; f_yy = yy; f_yz = yz;
            f_zx = zx; f_zy = zy; f_zz = zz;
        }

        public static implicit operator Matrix(Basis3D b)
        {
            Contract.Ensures(Contract.Result<Matrix>().M == 3, "Число столбцов матрицы = 3");
            Contract.Ensures(Contract.Result<Matrix>().N == 3, "Число строк матрицы = 3");

            //var M = new Matrix(3);
            //M[0, 0] = b.f_xx; M[0, 1] = b.f_xy; M[0, 2] = b.f_xz;
            //M[1, 0] = b.f_yx; M[1, 1] = b.f_yy; M[1, 2] = b.f_yz;
            //M[2, 0] = b.f_zx; M[2, 1] = b.f_zy; M[2, 2] = b.f_zz;

            //Contract.Ensures(M.IsSquare);
            //Contract.Ensures(M.N == 3);
            //Contract.Ensures(M.M == 3);
            //return M;

            return new Matrix(new[,]
                              {
                                  { b.f_xx, b.f_xy, b.f_xz }, 
                                  { b.f_yx, b.f_yy, b.f_yz }, 
                                  { b.f_zx, b.f_zy, b.f_zz }
                              });
        }

        public static explicit operator Basis3D(Matrix M)
        {
            Contract.Requires<ArgumentException>(M.M == 3 && M.N == 3, "Матрица должна быть размера 3х3");

            //if(M.M != 3 || M.N != 3)
            //    throw new ArgumentException("Матрица должна быть размера 3х3");
            //Contract.EndContractBlock();

            return new Basis3D(
                M[0, 0], M[0, 1], M[0, 2],
                M[1, 0], M[1, 1], M[1, 2],
                M[2, 0], M[2, 1], M[2, 2]);
        }
    }
}